#include <vector>
#include <math.h>
#include <iostream>

double distance(const std::vector<double> & v1, const std::vector<double> & v2)
{
    
    return sqrt(pow((v1[0] - v2[0]),2 ) + pow(v1[1] - v2[1], 2));
}

bool myfind(const std::vector<unsigned int> & v, const unsigned int & num)
{
    bool f = false;
    for (int i=0; i<v.size();i++)
    {
        if(v.at(i) == num)
        {
            f = true;
            break;
        }
    }

    return f;
}

double calResidual(const std::vector< std::vector<double>> & v1, const std::vector< std::vector<double>> & v2)
{
    if(v1.size() != v2.size())
    {
        std::cout << "Incompatable size of d_current and d_previous!" << std::endl;
        system("pause");
    }
    double res = 0.0;
    for(int i = 0; i<v2.size(); i++)
    {
        res+=distance(v1[i], v2[i]);
    }
    return res;
}